//
// Created by 13562 on 2024/2/3.
//
#include <csignal>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include "ImuHandler.h"
#include "Chrono.h"

#define NODE_NAME "imu_publisher"
#define PR_FILE "/imu_publisher/data_source"
#define PR_TOPIC "/imu_publisher/pub_name"
#define PR_DURATION "/imu_publisher/duration_sec"
#define PR_TOTAL "/imu_publisher/total"
#define PR_WAIT "/imu_publisher/wait"
#define PR_ALIGN "/imu_publisher/align"

#define TIMEOUT 10

ImuHandler ih;
bool g_halt = false;
std::chrono::milliseconds curr_ts;
std::chrono::milliseconds delay;
double dpf = 0;
uint dpf_ms = 0;


void sigint_handler(int sig) {
	if (sig == SIGINT) {
		std::cout << "Aborting by Ctrl-C" << std::endl;
		ih.halt();
		g_halt = true;
	}
}


void align_callback(const sensor_msgs::ImageConstPtr &msg) {
	std::chrono::seconds alin_secs(msg->header.stamp.sec);
	std::chrono::nanoseconds alin_nsecs(msg->header.stamp.nsec);
	std::chrono::milliseconds alin_ts = std::chrono::duration_cast<std::chrono::milliseconds>(alin_secs + alin_nsecs);
	delay = std::chrono::duration_cast<std::chrono::milliseconds>(curr_ts - alin_ts);

	if (delay.count() > 20) dpf_ms += 1;
	else if (delay.count() < -20) dpf_ms -= 1;
	else dpf_ms = uint(dpf*1000);
//	ROS_INFO("dpf: %d, delay: %ld", dpf_ms, delay.count());
}


int main(int argc, char** argv) {
	ros::init(argc, argv, NODE_NAME);
	ros::NodeHandle nh;
	ros::Subscriber alignSub;
	std::string pr_file, pr_topic, pr_wait, pr_align;
	int pr_duration, pr_total;
	nh.getParam(PR_FILE, pr_file);
	nh.getParam(PR_TOPIC, pr_topic);
	nh.getParam(PR_DURATION, pr_duration);
	nh.getParam(PR_TOTAL, pr_total);
	nh.getParam(PR_WAIT, pr_wait);
	nh.getParam(PR_ALIGN, pr_align);
	double fps = double(pr_total) / double(pr_duration);
	dpf = 1.0/fps;
	dpf_ms = uint(dpf*1000);

	uint max_try = TIMEOUT/dpf;

	std::cout << "IMU freq: " << fps << "(fps) | " << dpf_ms << "(dpf)" << std::endl;
	ros::Publisher publisher = nh.advertise<sensor_msgs::Imu>(pr_topic, 2);
	if (!pr_align.empty()) {
		ROS_WARN("Clock align to %s", pr_align.c_str());
		alignSub = nh.subscribe(pr_align, 2, align_callback);
	}

	using DS = DataSeries;
	std::vector<DataSeries> ds = {
			DS::ts,
			DS::ang_x,
			DS::ang_y,
			DS::ang_z,
			DS::acc_x,
			DS::acc_y,
			DS::acc_z
	};
	MsgInfo msgInfo = {
			"imu4",
			ds,
			{0,  0, 0, 0, 0, 0, 0 },
			8150,
			0
	};

	Chrono chrono;
	std::chrono::duration<long, std::milli> timeCost{};
	ih.handle(pr_file, msgInfo);
	signal(SIGINT, sigint_handler);
	bool enable = false;
	ros::Rate loop_rate(fps);

	while (ros::ok()) {
		if ((chrono.timeCostMilli() - timeCost) < std::chrono::milliseconds(dpf_ms)) continue;
		timeCost = chrono.timeCostMilli();

		unsigned long ql = ih.getQueueLength();
		ros::param::get(pr_wait, enable);
		if (ql <= 0) break;
		if (g_halt) break;

		if (!enable) {
			if (!max_try) {
				printf("No more times to try. Exit.");
				break;
			}
			printf("\rWaiting for /image_ok. Remain try: %u", max_try);
			continue;
		}

		sensor_msgs::Imu msg = ih.popMsg();
		msg.header.frame_id = std::to_string(ih.count());
		std::chrono::seconds curr_secs(msg.header.stamp.sec);
		std::chrono::nanoseconds curr_nsecs(msg.header.stamp.nsec);
		curr_ts = std::chrono::duration_cast<std::chrono::milliseconds>(curr_secs + curr_nsecs);
		std::cout << std::setw(18) << std::setfill(' ') << "\rimu ts(ms): " << curr_ts.count();
		std::cout << "  delay to align(ms): " << delay.count();
		publisher.publish(msg);
		ros::spinOnce();
	}
	ROS_INFO("\nMain thread exited.");
}